#导入模块
from typing import List, Tuple
from maix import camera, err,image,display,app,uart, touchscreen,gpio,pinmap ,key
from maix._maix.image import COLOR_BLACK, COLOR_BLUE, COLOR_GREEN, COLOR_RED, COLOR_YELLOW, Format
import math , struct
import numpy as np
import cv2,math

#类
class Laser:
    def __init__(self,thresholds:List[List[int]],err_threshold = 5,err = 5):
        self.now_position = ()         # 激光现在的位置
        self.last_position = ()        # 激光上一次的位置
        self.max_deviation = err       # 激光误识别误差像素
        self.target_position = ()      # 激光目标的位置
        self.index = 0                 # 激光目标位置的索引
        self.points = ()               # 激光目标位置的集合
        self.thresholds = thresholds   # 激光的阈值
        self.err_threshold = err_threshold # 激光追踪的误差值
        self.done = False              # 激光追踪完成标志位

    def set_points(self,points:Tuple):
        self.points = points

    def traceing_point(self,img:image.Image,target_point:Tuple,fact_position):
        if target_point:
            ex = 0
            ey = 0
            if not fact_position :
                return (5000,5000)
            img.draw_keypoints([int(target_point[0]),int(target_point[1])],color=COLOR_YELLOW)
            # 计算当前误差
            ex = target_point[0] - fact_position[0]
            ey = target_point[1] - fact_position[1]
            ex = int(-ex)
            ey = int(-ey)
            return ex , ey
        else :
            return 0,0

    def reset_task(self):
        self.done = False
        self.index = 0

    def traceing_box(self,img:image.Image,fact_position):
        if self.done or self.index >= len(self.points):   # 判断是否完成所有点的追踪
            self.done = True
            if self.points :
                return self.traceing_point(img,self.points[0],fact_position)

        ex = 0 
        ey = 0
        # 获取当前目标点
        if self.index < len(self.points):
            target_point = self.points[self.index]
            img.draw_keypoints([int(target_point[0]),int(target_point[1])],color=COLOR_YELLOW)
            print(int(target_point[0]),int(target_point[1]))

            ex,ey = self.traceing_point(img,target_point,fact_position)
            #对误差进行判断
            if -self.err_threshold < ex < self.err_threshold and -self.err_threshold < ey < self.err_threshold:
                self.index = self.index + 1
            
            self.done = ( self.index >= len(self.points) )

        return ex , ey
    
    def get_laser_position(self,img:image.Image,target_threshold:list[list[int]]):
        blobs = img.find_blobs(thresholds=target_threshold,area_threshold=3,pixels_threshold=3)
        if blobs:
            blob = max(blobs , key = lambda blob:blob.area()) #找最大的色块
            img.draw_rect(blob[0],blob[1],blob[2],blob[3],color=COLOR_GREEN)
            #获得激光现在的位置
            self.now_position = (
                blob.cx(),
                blob.cy()
            )
            img.draw_cross(self.now_position[0],self.now_position[1],color=COLOR_GREEN,size=5)
            
            print(self.now_position)
            return self.now_position

    '''
    def get_laser_position(self, img: image.Image, target_threshold: list[list[int]], max_deviation: int = 6):
    # 检测当前帧激光点
        blobs = img.find_blobs(thresholds=target_threshold, area_threshold=3, pixels_threshold=3, merge=True)
        current_position = None
        if blobs:
            blob = max(blobs, key=lambda b: b.area())
            img.draw_keypoints([blob.cx(),blob.cy()],COLOR_GREEN,size= 8)
            current_position = (blob.cx(), blob.cy())
            print(current_position)
            
    
        # 情况1：无激光点 → 返回None（明确丢失）
        if not current_position :
            print("情况1")
            return None
    
        # 情况2：首次检测 → 初始化位置
        if not self.last_position :
            print("情况2")
            self.last_position = current_position
            self.now_position = current_position
            return current_position

        if self.last_position:
        # 情况3：位移校验
            print("情况3")
            dx = current_position[0] - self.last_position[0]
            dy = current_position[1] - self.last_position[1]
            distance_sq = dx**2 + dy**2
    
            if distance_sq <= max_deviation**2:  # 位移在容差内
                self.now_position = current_position
                self.last_position = current_position  # 更新历史位置
                return current_position
            else:  # 位移超限 → 视为干扰，返回历史位置
                print(f"位移超限！偏差{math.sqrt(distance_sq):.1f}像素 > {max_deviation}")
                return self.last_position  # 关键修改：返回历史位置而非None
    '''

def uart_send(serial:uart.UART,head,data:list):
    send_data = struct.pack("<Bhh",head,data[0],data[1])
    serial.write(send_data)


cam = camera.Camera()
disp = display.Display()
device1 = "/dev/ttyS0"          # 示例的串口名称                        此串口设备名称为串口0
serial = uart.UART(device1,115200)  #创建串口对象，八位数据，无检验，一位停止
ts = touchscreen.TouchScreen()      #创建触摸屏对象

red_laser = Laser(thresholds=[[40, 100, -19, 54, -24, 84]],err_threshold = 10)  # 红色激光实例
green_laser = Laser(thresholds=[[35, 100, -58, -15, -30, 13]],err_threshold = 10) # 绿色激光实例

'''画面处理部分'''
cam.luma(15)  # 调暗摄像头的亮度
cam.constrast(80)		# 设置对比度，范围[0, 100]
cam.saturation(70)		# 设置饱和度，范围[0, 100]
cam.exp_mode(0)


red_position = ()
green_position = ()


cam.skip_frames(150)           # 跳过开头的30帧

while not app.need_exit():
    img = cam.read()
    
    img = img.crop(180,70,260,200)  # 将照片放大
    #img.draw_string(0,0,"the versions is test")  # 对照版本号
    
    
    ex = 0
    ey = 0
    red_position = red_laser.get_laser_position(img,red_laser.thresholds)
    green_position = green_laser.get_laser_position(img,green_laser.thresholds)
    if red_position and green_position:
        ex = red_position[0] - green_position[0]
        ey = red_position[1] - green_position[1]
    elif red_position :
        print("红色激光没有追踪到")
    else :
        print("绿色激光没有追踪到")
    '''
    green_position = green_laser.get_laser_position(img,green_laser.thresholds)
    ex,ey = green_laser.traceing_point(img,(int(img.width()/2),int(img.height()/2)),green_position)
    
    #刘嘉健是傻逼
    
    '''
    print(ex,ey)
    #img.draw_string(0,40,f"ex={ex},ey={ey}")  # 对照版本号
    uart_send(serial,0x55,[ex,ey])
    img.save("/root/test/image.jpg")
    disp.show(img)
